﻿#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <QVBoxLayout>
#include <QFileDialog>
#include <QDir>
#include <QDebug>
#include <QLabel>
#include "iostream"

#include <QWidget>

#include <osgDB/ReadFile>
#include <osgViewer/Viewer>
#include <osgGA/AnimationPathManipulator>
#include <osgGA/DriveManipulator>
#include <osgGA/FlightManipulator>
#include <osgGA/KeySwitchMatrixManipulator>
#include <osgGA/SphericalManipulator>
#include <osgGA/StateSetManipulator>
#include <osgGA/TerrainManipulator>
#include <osgGA/TrackballManipulator>
#include <osgViewer/ViewerEventHandlers>
#include <osgUtil/Optimizer>
#include <osg/Material>
#include <osg/Geode>
#include <osgUtil/DelaunayTriangulator>
#include <osg/Quat>


osg::ref_ptr<osg::Geode> createColoredSTLModel(const std::string& filename, const osg::Vec4& color) {
    osg::ref_ptr<osg::Geode> geode = new osg::Geode();
    osg::ref_ptr<osg::Node> model = osgDB::readNodeFile(filename);
    if (model && model->asGroup()) {
        osg::Group* modelGroup = model->asGroup();
        for (unsigned int i = 0; i < modelGroup->getNumChildren(); ++i) {
            osg::Geode* modelGeode = dynamic_cast<osg::Geode*>(modelGroup->getChild(i));
            if (modelGeode) {
                for (unsigned int j = 0; j < modelGeode->getNumDrawables(); ++j) {
                    osg::Geometry* geom = dynamic_cast<osg::Geometry*>(modelGeode->getDrawable(j));
                    if (geom) {
                        osg::Vec3Array* vertices = dynamic_cast<osg::Vec3Array*>(geom->getVertexArray());
                        osg::Vec4Array* colors = new osg::Vec4Array;
                        colors->push_back(color);
                        geom->setColorArray(colors);
                        geom->setColorBinding(osg::Geometry::BIND_OVERALL);

                        // Optionally, you can use DelaunayTriangulator to improve the model's rendering
                        //                        osgUtil::DelaunayTriangulator t;
                        //                        t.triangulateConvexPolygon(*vertices);
                        geom->setVertexArray(vertices);

                        geode->addDrawable(geom);
                    }
                }
            }
        }
    }
    return geode.get();
}

MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    //d3widget= new osgQOpenGLWidget(this);
    QVBoxLayout* layout=new QVBoxLayout(ui->frame);
    //layout->addWidget(d3widget);
    pOsgW = new osgQOpenGLWidget;
    layout->addWidget(pOsgW);
    connect(pOsgW, SIGNAL(initialized()), this, SLOT(initosg()));
    reader=elementReader::Instance();

}

MainWindow::~MainWindow()
{
    delete ui;
}


void MainWindow::on_pushButton_clicked()
{
    QString path = QFileDialog::getExistingDirectory(this,"open stl models","E:/vscord/winrobo/bin/Robot/Models");
    if(path.isEmpty())
        return;
    QDir dir(path);
    path=dir.fromNativeSeparators(path);
    QStringList list;
    if(!dir.exists())
        list=QStringList("");
    dir.setFilter(QDir::Files);
    dir.setSorting(QDir::Name);
    dir.setNameFilters({"*.stl","*.STL"});
    list=dir.entryList();
    osg::ref_ptr<osg::Node> pNode = 0;
    for(int i=0;i<list.size();++i)
    {
        QString str=path+"/"+list.at(i);
        qDebug()<<"entryList"<<i<<"_"<<path+"/"+list.at(i);
        osg::ref_ptr<osg::Node> pNode = osgDB::readNodeFile(str.toStdString());
        QString name=list.at(i).left(list.at(i).length()-4);
        nodemap[name]=pNode;
        //        osgUtil::Optimizer optimizer;
        //        optimizer.optimize(pNode);
        //        pOsgW->getOsgViewer()->setSceneData(pNode);
    }
    qDebug()<<nodemap.keys();
}

void MainWindow::initosg()
{
    osgViewer::Viewer* pViewer = ((osgQOpenGLWidget *)sender())->getOsgViewer();
    osg::ref_ptr<osgGA::KeySwitchMatrixManipulator> keyswitchManipulator = new osgGA::KeySwitchMatrixManipulator;
    keyswitchManipulator->addMatrixManipulator('1', "Trackball", new osgGA::TrackballManipulator());
    keyswitchManipulator->addMatrixManipulator('2', "Flight", new osgGA::FlightManipulator());
    keyswitchManipulator->addMatrixManipulator('3', "Drive", new osgGA::DriveManipulator());
    keyswitchManipulator->addMatrixManipulator('4', "Terrain", new osgGA::TerrainManipulator());
    keyswitchManipulator->addMatrixManipulator('5', "Orbit", new osgGA::OrbitManipulator());
    keyswitchManipulator->addMatrixManipulator('6', "FirstPerson", new osgGA::FirstPersonManipulator());
    keyswitchManipulator->addMatrixManipulator('7', "Spherical", new osgGA::SphericalManipulator());
    pViewer->setCameraManipulator(keyswitchManipulator.get());
    // add the state manipulator

    pViewer->addEventHandler(new osgGA::StateSetManipulator(pViewer->getCamera()->getOrCreateStateSet()));
    pViewer->addEventHandler(new osgViewer::StatsHandler);


}


void MainWindow::on_checkBox_stateChanged(int arg1)
{
    pOsgW->showfloor(ui->checkBox->isChecked());
}

void printmatrix(osg::Matrix matrix)
{
    qDebug()<<matrix(0,0)<<matrix(0,1)<<matrix(0,2)<<matrix(0,3)<<"\n"
             <<matrix(1,0)<<matrix(1,1)<<matrix(1,2)<<matrix(1,3)<<"\n"
             <<matrix(2,0)<<matrix(2,1)<<matrix(2,2)<<matrix(2,3)<<"\n"
             <<matrix(3,0)<<matrix(3,1)<<matrix(3,2)<<matrix(3,3);
}

void MainWindow::on_pushButton_2_clicked()
{
    QString dir=qApp->applicationDirPath();
    qDebug()<<dir;
    QString path = QFileDialog::getOpenFileName(this,"getfile",qApp->applicationDirPath()+"/../","XML-File(*.xml)");
    reader->getInitilizationFile(path);

    QMap<QString,JOINT> map=reader->getdatamap();
    if(map.size()<1)
        return;
    QStringList list;
    QString pernode= findKey("null",map);
    if(!pernode.isEmpty())
        list<<pernode;
    for(int i=1;i<map.size();++i)
    {
        QString last=list.last();
        last=findKey(last,map);
        list.push_back(last);
    }
    if(osgnodelist.size()!=0)
        return;
    jointdata=QVector<double>(list.size(),0.0);
    QVBoxLayout* joglayout= new QVBoxLayout(ui->groupBox_jog);
    QVBoxLayout* coordlayout= new QVBoxLayout(ui->groupBox_coord);
    QString firstnode;
    for(int i=0;i<list.size();++i)
    {
        QString key=list.at(i);
        osg::ref_ptr<osg::MatrixTransform> trans=new osg::MatrixTransform;
        double roll= osg::inDegrees(map[key].rpy.z());
        double pitch= osg::inDegrees(map[key].rpy.y());
        double yaw= osg::inDegrees(map[key].rpy.x());
        osg::Quat rollQuat = osg::Quat(roll, osg::Vec3(1.0f, 0.0f, 0.0f));
        osg::Quat pitchQuat = osg::Quat( pitch,osg::Vec3(0.0f, 1.0f, 0.0f));
        osg::Quat yawQuat = osg::Quat(yaw,osg::Vec3(0.0f, 0.0f, 1.0f));
        osg::Vec3 Pos(map[key].pos.x(),map[key].pos.y(),map[key].pos.z());
//        jogmap[key].matrix= osg::Matrix(osg::Matrix::translate(map[key].pos.x(),map[key].pos.y(),map[key].pos.z())*osg::Matrix::rotate(osg::inDegrees(map[key].rpy.x()),1,0,0)*osg::Matrix::rotate(osg::inDegrees(map[key].rpy.y()),0,1,0)*
//                                 osg::Matrix::rotate(osg::inDegrees(map[key].rpy.z()),0,0,1));
        jogmap[key].matrix= osg::Matrix(osg::Matrix::rotate(rollQuat)*osg::Matrix::rotate(pitchQuat)*osg::Matrix::rotate(yawQuat)*osg::Matrix::translate(Pos));
        jogmap[key].model=new osg::Group;
        for(int i=0;i<map[key].modelpath.size();++i)
        {
            std::string pth=map[key].modelpath.at(i).path.toStdString();
            QVector4D color= map[key].modelpath.at(i).color;
            osg::ref_ptr<osg::Geode> geode = createColoredSTLModel(pth,osg::Vec4(color.x(),color.y(),color.z(),color.w()));
            jogmap[key].model->addChild(geode);
        }
        osg::ref_ptr<osg::Node> axisnode = osgDB::readRefNodeFile("axes.osgt");
        osg::ref_ptr<osg::MatrixTransform> axis = new osg::MatrixTransform;
        osg::Matrix axismatrix=osg::Matrix::scale(100,100,100);
        axis->addChild(axisnode);
        axis->setMatrix(axismatrix);
        jogmap[key].coord=axis;
        jogmap[key].coord->setNodeMask(0);
        //trans->addChild(jogmap[key].coord);
        jogmap[key].trans=trans;
        QString parent=map[key].prenode;
        //qDebug()<<"parent node "<<parent<<key;
        QCheckBox* check=new QCheckBox(key);
        check->setChecked(false);
        coordlayout->addWidget(check);
        connect(check,SIGNAL(stateChanged(int)),this,SLOT(showcoord(int)));
        if(parent=="null" || map[key].type==Fixed)
        {
            firstnode=key;
            jogmap[key].trans = trans;
        }
        else
        {
            QLabel* nameinfo=new QLabel(key);
            QPushButton* reduce=new QPushButton("<");
            jogmap[key].Leftbt=reduce;
            reduce->setAutoRepeat(true);
            reduce->setFixedSize(30,30);
            reduce->setObjectName(key+"L");
            QPushButton* add=new QPushButton(">");
            jogmap[key].Righbt=add;
            add->setFixedSize(30,30);
            add->setAutoRepeat(true);
            add->setObjectName(key+"R");
            QDoubleSpinBox* vld=new QDoubleSpinBox(this);
            jogmap[key].spbox=vld;
            qDebug()<<map[key].min<<map[key].max;
            vld->setMinimum(map[key].min);
            vld->setMaximum(map[key].max);
            connect(reduce,&QPushButton::pressed,this,&MainWindow::jointdatachange);
            connect(add,&QPushButton::pressed,this,&MainWindow::jointdatachange);
            connect(vld,SIGNAL(valueChanged(double)),this,SLOT(jogjoint(double)));
            osgnodelist[vld]=trans;
            QHBoxLayout* hlayout=new QHBoxLayout();
            hlayout->addWidget(nameinfo);
            hlayout->addWidget(reduce);
            hlayout->addWidget(vld);
            hlayout->addWidget(add);
            joglayout->addLayout(hlayout);
            //jogmap[key].trans = trans;
        }
    }
    osg::Matrix basematrix = jogmap["Base"].matrix;
    jogmap["Base"].trans->setMatrix(basematrix);
    jogmap["Base"].trans->addChild(jogmap["Base"].model);
    jogmap["Base"].trans->addChild(jogmap["Base"].coord);
    pOsgW->Display(jogmap["Base"].trans);
    osg::Matrix j1matrix = jogmap["Joint1"].matrix*basematrix;
    jogmap["Joint1"].trans->setMatrix(j1matrix);
    jogmap["Joint1"].trans->addChild(jogmap["Joint1"].model);
    jogmap["Joint1"].trans->addChild(jogmap["Joint1"].coord);    
    pOsgW->Display(jogmap["Joint1"].trans);
    j1matrix.preMult(jogmap["Joint2"].matrix);
    jogmap["Joint2"].trans->setMatrix(j1matrix);
    jogmap["Joint2"].trans->addChild(jogmap["Joint2"].model);
    jogmap["Joint2"].trans->addChild(jogmap["Joint2"].coord);
    pOsgW->Display(jogmap["Joint2"].trans);
    j1matrix.preMult(jogmap["Joint3"].matrix);
    jogmap["Joint3"].trans->setMatrix(j1matrix);
    jogmap["Joint3"].trans->addChild(jogmap["Joint3"].model);
    jogmap["Joint3"].trans->addChild(jogmap["Joint3"].coord);
    pOsgW->Display(jogmap["Joint3"].trans);
    j1matrix.preMult(jogmap["Joint4"].matrix);
    jogmap["Joint4"].trans->setMatrix(j1matrix);
    jogmap["Joint4"].trans->addChild(jogmap["Joint4"].model);
    jogmap["Joint4"].trans->addChild(jogmap["Joint4"].coord);
    pOsgW->Display(jogmap["Joint4"].trans);
    j1matrix.preMult(jogmap["Joint5"].matrix);
    jogmap["Joint5"].trans->setMatrix(j1matrix);
    jogmap["Joint5"].trans->addChild(jogmap["Joint5"].model);
    jogmap["Joint5"].trans->addChild(jogmap["Joint5"].coord);
    pOsgW->Display(jogmap["Joint5"].trans);
    j1matrix.preMult(jogmap["Joint6"].matrix);
    jogmap["Joint6"].trans->setMatrix(j1matrix);
    jogmap["Joint6"].trans->addChild(jogmap["Joint6"].model);
    jogmap["Joint6"].trans->addChild(jogmap["Joint6"].coord);
    pOsgW->Display(jogmap["Joint6"].trans);
    j1matrix.preMult(jogmap["TCP"].matrix);
    jogmap["TCP"].trans->setMatrix(j1matrix);
    jogmap["TCP"].trans->addChild(jogmap["TCP"].coord);
    pOsgW->Display(jogmap["TCP"].trans);
}

void MainWindow::jointdatachange()
{
    QPushButton* button= static_cast<QPushButton*>(this->sender());
    QString str=button->objectName();
    QString joint=str.left(str.length()-1);
    if(jogmap.contains(joint))
    {
        double vl=jogmap[joint].spbox->value();
        if(button==jogmap[joint].Leftbt)
        {
            vl--;
        }else
        {
            vl++;
        }
        jogmap[joint].spbox->setValue(vl);
    }
}

void MainWindow::jogjoint(double value)
{
    QDoubleSpinBox* spb=static_cast<QDoubleSpinBox*>(this->sender());

    for(int i=0;i<jogmap.keys().size();++i)
    {
        QString key=jogmap.keys().at(i);
        if(jogmap[key].spbox==spb)
        {
            jointdata[i]=value;
        }
    }
    viewupdate();
}

void MainWindow::showcoord(int isshow)
{
    QCheckBox* box=static_cast<QCheckBox*>(this->sender());
    QString name=box->text();
    qDebug()<<"showcoord"<<name;
    bool ischeck=box->isChecked();
    jogmap[name].coord->setNodeMask(ischeck?1:0);
    //pOsgW->showobject(ischeck,jogmap[name].coord);
}

QString MainWindow::findKey(QString key,QMap<QString,JOINT> map)
{
    foreach(QString name,map.keys())
    {
        if(map[name].prenode==key)
            return name;
    }
}

void MainWindow::viewupdate()
{
    qDebug()<<"current joint == "<<jointdata;
    osg::Matrix basematrix= jogmap["Base"].matrix;
    //basematrix.preMult(osg::Matrix::rotate(osg::inDegrees(jointdata.at(0)),0,0,1));
    //basematrix=basematrix.rotate(osg::inDegrees(jointdata.at(0)),0,0,1);
    jogmap["Base"].trans->setMatrix(basematrix);

    osg::Matrix j1matrix = jogmap["Joint1"].matrix;
    j1matrix.preMult(osg::Matrix::rotate(osg::inDegrees(jointdata.at(1)),0,0,1));
    basematrix.preMult(j1matrix);
    jogmap["Joint1"].trans->setMatrix(basematrix);

    osg::Matrix j2matrix= jogmap["Joint2"].matrix;
    j2matrix.preMult(osg::Matrix::rotate(osg::inDegrees(jointdata.at(2)),0,0,1));
    basematrix.preMult(j2matrix);
    jogmap["Joint2"].trans->setMatrix(basematrix);

    osg::Matrix j3matrix= jogmap["Joint3"].matrix;
    j3matrix.preMult(osg::Matrix::rotate(osg::inDegrees(jointdata.at(3)),0,0,1));
    basematrix.preMult(j3matrix);
    jogmap["Joint3"].trans->setMatrix(basematrix);

    osg::Matrix j4matrix= jogmap["Joint4"].matrix;
    j4matrix.preMult(osg::Matrix::rotate(osg::inDegrees(jointdata.at(4)),0,0,1));
    basematrix.preMult(j4matrix);
    jogmap["Joint4"].trans->setMatrix(basematrix);

    osg::Matrix j5matrix= jogmap["Joint5"].matrix;
    j5matrix.preMult(osg::Matrix::rotate(osg::inDegrees(jointdata.at(5)),0,0,1));
    basematrix.preMult(j5matrix);
    jogmap["Joint5"].trans->setMatrix(basematrix);

    osg::Matrix j6matrix= jogmap["Joint6"].matrix;
    j6matrix.preMult(osg::Matrix::rotate(osg::inDegrees(jointdata.at(6)),0,0,1));
    basematrix.preMult(j6matrix);
    jogmap["Joint6"].trans->setMatrix(basematrix);

    osg::Matrix tcpmatrix= jogmap["TCP"].matrix;
    //tcpmatrix.preMult(osg::Matrix::rotate(osg::inDegrees(jointdata.at(6)),0,0,1));
    basematrix.preMult(tcpmatrix);
    jogmap["TCP"].trans->setMatrix(basematrix);
    pOsgW->update();
}

